#pragma once
class Matrix;

class Quaternion
{
public:
	static const Quaternion ZERO;
	static const Quaternion Identity;
	float X, Y, Z, W;

	Quaternion() {};
	Quaternion(float x, float y, float z, float w) : X(x), Y(y), Z(z), W(w) {};
	Quaternion(const Matrix& m);
	Quaternion(const Vector3& Axis, float Angle )
	{
		const FLOAT half_a = 0.5f * Angle;
		const FLOAT s = sin(half_a);
		const FLOAT c = cos(half_a);

		X = s * Axis.X;
		Y = s * Axis.Y;
		Z = s * Axis.Z;
		W = c;
	}

	Quaternion operator+( const Quaternion& V ) const { return Quaternion( X + V.X, Y + V.Y, Z + V.Z, W + V.W ); }
	Quaternion operator-( const Quaternion& V ) const { return Quaternion( X - V.X, Y - V.Y, Z - V.Z, W - V.W ); }
	Quaternion operator*(float Scale) const { return Quaternion( X * Scale, Y * Scale, Z * Scale, W * Scale ); }

	float operator|(const Quaternion& Q) const { return X*Q.X + Y*Q.Y + Z*Q.Z + W*Q.W; }

	Quaternion operator*( const Quaternion& Q ) const;
	Quaternion operator*=(const Quaternion& Q);
	Vector3 operator*(const Vector3& v) const;

	void ToRotationMatrix(class Matrix3& kRot) const;

    Quaternion Inverse() const;
    Quaternion UnitInverse () const;

	void Normalize();
};

inline Quaternion Quaternion::operator*( const Quaternion& Q ) const
{
	Quaternion Result = *this;
	return (Result *= Q);
}

inline Quaternion Quaternion::operator*=(const Quaternion& Q)
{
	const float T0 = (Z - Y) * (Q.Y - Q.Z);
	const float T1 = (W + X) * (Q.W + Q.X);
	const float T2 = (W - X) * (Q.Y + Q.Z);
	const float T3 = (Y + Z) * (Q.W - Q.X);
	const float T4 = (Z - X) * (Q.X - Q.Y);
	const float T5 = (Z + X) * (Q.X + Q.Y);
	const float T6 = (W + Y) * (Q.W - Q.Z);
	const float T7 = (W - Y) * (Q.W + Q.Z);
	const float T8 = T5 + T6 + T7;
	const float T9 = 0.5f * (T4 + T8);

	X = T1 + T9 - T8;
	Y = T2 + T9 - T7;
	Z = T3 + T9 - T6;
	W = T0 + T9 - T5;

	return *this;
}

inline Vector3 Quaternion::operator*(const Vector3& v) const
{
	const Vector3 qv(X, Y, Z);
	Vector3 vOut = (qv ^ v) * 2.f * W;
	vOut += v * ((W * W) - (qv | qv));
	vOut += qv * (2.f * (qv | v));
	return vOut;
}
	
//-----------------------------------------------------------------------
inline Quaternion Quaternion::Inverse() const
{
    float fNorm = X * X + Y * Y + Z * Z + W * W;
    if (fNorm > 0.0)
    {
        float fInvNorm = 1.0f / fNorm;
        return Quaternion(-X * fInvNorm, -Y * fInvNorm, -Z * fInvNorm, W * fInvNorm);
    }
    else
    {
        // return an invalid result to flag the error
        return ZERO;
    }
}
//-----------------------------------------------------------------------
inline Quaternion Quaternion::UnitInverse () const
{
    // assert:  'this' is unit length
    return Quaternion(-X, -Y, -Z, W);
}
	
inline void Quaternion::Normalize()
{
	const FLOAT SquareSum = X*X + Y*Y + Z*Z + W*W;
	if( SquareSum > SMALL_NUMBER )
	{
		const FLOAT Scale = appInvSqrt(SquareSum);
		X *= Scale; 
		Y *= Scale; 
		Z *= Scale;
		W *= Scale;
	}
	else
	{
		*this = Quaternion::Identity;
	}
}

inline Quaternion LerpQuat(const Quaternion& A, const Quaternion& B, const float Alpha)
{
	if ((A | B) < 0.f)
		return (B * Alpha) - (A * (1.f - Alpha));
	return (B * Alpha) + (A * (1.f - Alpha));
}
